// #include "zf_common_headfile.h"
// #define Motor_Max_Duty 180

// #include <stdio.h>
// #include <math.h>

// SpeedPIDController speed_pidA,speed_pidB;
// // 初始化PID控制器
// void SpeedPID_Init(SpeedPIDController *pid, float Kp, float Ki, float Kd, float output_limit) 
// {
//     pid->Kp = Kp;
//     pid->Ki = Ki;
//     pid->Kd = Kd;
//     pid->output_limit = output_limit;
    
//     // 重置所有状态变量
//     pid->integral = 0.0f;
//     pid->prev_error = 0.0f;
//     pid->prev_measurement = 0.0f;
// }

// // 计算PID输出 (目标速度, 实际速度)
// volatile int16 SpeedPID_Compute(SpeedPIDController *pid, volatile int16 target_speed, volatile int16 actual_speed) 
// {
//     // 1. 计算当前误差
//     volatile int16 error = target_speed - actual_speed;
    
//     // 2. 比例项
//     volatile int16 P = pid->Kp * error;
    
//     // 3. 积分项 (带抗饱和处理)
//     pid->integral += error;  // dt由定时器中断周期隐含
    
//     // 积分限幅抗饱和
//     if (pid->Ki != 0.0f) {
//         // 计算最大允许积分值
//         volatile int16 max_integral = fabsf(pid->output_limit / pid->Ki);
//         // 限制积分项范围
//         if (pid->integral > max_integral) {
//             pid->integral = max_integral;
//         } else if (pid->integral < -max_integral) {
//             pid->integral = -max_integral;
//         }
//     }
//     volatile int16 I = pid->Ki * pid->integral;
    
//     // 4. 微分项 (使用测量值微分减少冲击)
//     volatile int16 D = -pid->Kd * (actual_speed - pid->prev_measurement);  // dt由定时器中断周期隐含
    
//     // 5. 计算总输出
//     volatile int16 output = P + I + D;
    
//     // 6. 输出限幅
//     if (output > pid->output_limit) {
//         output = pid->output_limit;
//     } else if (output < -pid->output_limit) {
//         output = -pid->output_limit;
//     }
    
//     // 7. 更新状态
//     pid->prev_error = error;
//     pid->prev_measurement = actual_speed;
    
//     return output;
// }

// PID_t MotorA;
// PID_t MotorB;
// uint8_t MotorA_dir = 1,MotorB_dir = 1;
// extern uint32_t Speed_Left_Now;//电机速度
// extern uint32_t Speed_Right_Now;
// static int ControlVelocityA = 0;  // 控制输出值


// void Motor_Target_Set(float spe1, float spe2)
// {
// 	if(spe1 >= 0)
// 	{
// 		MotorA_dir = 1;
// 		MotorA.Target = spe1;
// 	}
// 	else
// 	{
// 		MotorA_dir = 0;
// 		MotorA.Target = -spe1;
// 	}
// 	if(spe2 >= 0)
// 	{
// 		MotorB_dir = 1;
// 		MotorB.Target = spe2;
// 	}
// 	else
// 	{
// 		MotorB_dir = 0;
// 		MotorB.Target = -spe2;
// 	}
// }

// //volatile int16 PID_Update(PID_t *p,volatile int16 Tar,volatile int16 Act)
// //{
// //    //1. 获取本次误差和上次误差
// //    p->Error1 = p->Error0;
// //    p->Error0 = Tar - Act;
// //    //2./*外环误差积分（累加）*/
// //	/*如果Ki不为0，才进行误差积分，这样做的目的是便于调试*/
// //	/*因为在调试时，我们可能先把Ki设置为0，这时积分项无作用，误差消除不了，误差积分会积累到很大的值*/
// //	/*后续一旦Ki不为0，那么因为误差积分已经积累到很大的值了，这就导致积分项疯狂输出，不利于调试*/
// //    if(p->Ki != 0)
// //    {
// //        p->ErrorInt += p->Error0;
// //    }
// //    else 
// //    {
// //        p->ErrorInt = 0;
// //    }
// //    //3.PID计算
// //    //使用位置式公式
// //    if(p->Pid_mode == PID_POSITION)
// //    {
// //        p->Out = (p->Kp * p->Error0 + 
// //                 p->Ki * p->ErrorInt +
// //                 p->Kd * (p->Error0 - p->Error1));
// //    }
// //    if(p->Pid_mode == PID_DELTA)
// //    {
// //        p->Out = (p->Kp * (p->Error0 - p->Error1) + 
// //                 p->Ki * p->Error0 +
// //                 p->Kd * (p->Error0 - 2*p->Error1 + p->Error2));
// //    }
// //    //记录误差
// //    p->Error2 = p->Error1;
// //    //输出限幅
// //    
// //	// 更新控制输出值
// //    ControlVelocityA += p->Out;
// //	if(ControlVelocityA > p->OutMax)
// //    {
// //        ControlVelocityA = p->OutMax;
// //    }
// //    if(ControlVelocityA < p->OutMin)
// //    {
// //        ControlVelocityA = p->OutMin;
// //    }
// //	return ControlVelocityA;
// //}
// //void Pidout_Limit(PID_t *pid, volatile int16 Min, volatile int16 Max)
// //{
// //    pid->OutMin = Min;
// //    pid->OutMax = Max;
// //}

// //void Pid_Init(PID_t *pid, uint32_t mode, volatile int16 p, volatile int16 i, volatile int16 d)
// //{
// //	pid->Pid_mode = mode;
// //	pid->Kp = p;
// //	pid->Ki = i;
// //	pid->Kd = d;
// //}

// //void Pid_Cal(void)
// //{
// //    Trace2();
// //    //1.获取当前速度
// //	MotorA.Actual =abs(Encoder_Speed_L);
// //	MotorB.Actual =abs(Encoder_Speed_R);
// //	Encoder_Speed_L = Encoder_Count_A;
// //	Encoder_Speed_R = Encoder_Count_B;
// //	Encoder_Count_B = 0;
// //	Encoder_Count_A = 0;
// //	
// //	int pwmA = Velocity_A(MotorA.Target,MotorA.Actual);
// //	int pwmB = Velocity_B(MotorB.Target,MotorB.Actual);
// //	
// //    //2.计算PID
// //    //PID_Update(&MotorA);
// //    //PID_Update(&MotorB);
// //    //3.输出
// //	//MotorA_Duty(MotorA.Out);
// //    //MotorB_Duty(MotorB.Out);
// //	if(pwmA>5000)
// //	{
// //		pwmA = 5000;
// //	}
// //	else if(pwmA < -5000)
// //	{
// //		pwmA = -5000;
// //	}
// //	if(pwmB>5000)
// //	{
// //		pwmB = 5000;
// //	}
// //	else if(pwmB < -5000)
// //	{
// //		pwmB = -5000;
// //	}
// //	
// //	
// //	//Motor_Out(pwmA,pwmB);
// //	printf("targetSpeedA:%d\r\n",MotorA.Target);
// //	printf("targetSpeedA:%d\r\n",MotorA.Target);
// //	printf("MotorA.Out:%d\r\n",pwmA);
// //	printf("MotorB.Out:%d\r\n",pwmB);
// //    printf("Encoder_Count_A:%d\r\n", Encoder_Speed_L);
// //    printf("Encoder_Count_B:%d\r\n", Encoder_Speed_R);

// //}


// //void Pid_Cal(void)
// //{
// //	
// //}

